# !/usr/bin/python
#
# Example code to go through the hokuyo_30m.bin file, read timestamps and the hits
# in each packet, and plot them.
#
# To call:
#
#   python3 read_hokuyo_30m.py /media/gwm/152E776914CDED89/linux_work/open_dataset/nclt/20130110/hokuyo_30m.bin
#

import sys
import struct
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.colors import Normalize
from matplotlib.cm import ScalarMappable

def convert(x_s, scaling=0.005, offset=-100.0):
    return x_s * scaling + offset

def main(args):
    if len(args) < 2:
        print("Usage: python color_lidar_viewer.py <input.bin>")
        return 1

    NUM_HITS = 1081
    RAD_START = np.deg2rad(-135)
    RAD_STEP = np.deg2rad(0.25)
    angles = np.linspace(RAD_START, RAD_START + (NUM_HITS-1)*RAD_STEP, NUM_HITS)

    try:
        with open(args[1], "rb") as f_bin:
            plt.ion()
            fig, ax = plt.subplots(figsize=(10, 8))
            
            cmap = plt.get_cmap('viridis')
            norm = Normalize(vmin=0, vmax=30)
            plt.colorbar(ScalarMappable(norm=norm, cmap=cmap), ax=ax, label='Distance (m)')

            while True:
                timestamp = f_bin.read(8)
                if not timestamp: break
                
                utime = struct.unpack('<Q', timestamp)[0]
                ranges = np.fromfile(f_bin, dtype='<H', count=NUM_HITS)
                if len(ranges) != NUM_HITS: break
                
                distances = convert(ranges)
                x = distances * np.cos(angles)
                y = distances * np.sin(angles)
                
                ax.clear()
                ax.scatter(x, y, c=distances, cmap=cmap, norm=norm, s=5)  # 修正：添加了s=5
                ax.set_title(f'Scan at {utime} | Max: {np.max(distances):.1f}m')
                ax.set_xlabel('X (m)')
                ax.set_ylabel('Y (m)')
                ax.grid(True)
                ax.set_xlim(-30, 30)
                ax.set_ylim(-30, 30)
                ax.set_aspect('equal')
                
                plt.draw()
                plt.pause(0.1)

    except Exception as e:
        print(f"Error: {e}")
        return 1

    return 0

if __name__ == '__main__':
    sys.exit(main(sys.argv))
